/*
 * physics.cpp
 *
 *  Created on: 30.03.2011
 *      Author: inkooboo
 */

#include "physics.hpp"
#include "logger.hpp"
#include "f_engine.hpp"
#include "scheduler.hpp"
#include "object_manager.hpp"

boost::shared_ptr<if_system_t> physics_manager_t::create(f_engine_t *engine)
{
    boost::shared_ptr<physics_manager_t> ret(new physics_manager_t(engine));
    logger::log(DEBUG, "[Physics] Physics system created.");
    return ret;
}

physics_manager_t::physics_manager_t(f_engine_t *engine) :
    engine_(engine)
{
    //initialize bullet
    broadphase_.reset(new btDbvtBroadphase());

    collision_configuration_.reset(new btDefaultCollisionConfiguration());
    dispatcher_.reset(new btCollisionDispatcher(collision_configuration_.get()));

    solver_.reset(new btSequentialImpulseConstraintSolver);

    dynamics_world_.reset(new btDiscreteDynamicsWorld(dispatcher_.get(),broadphase_.get(),solver_.get(),collision_configuration_.get()));

    dynamics_world_->setGravity(btVector3(0,-10,0));

    //add ground model
    ground_shape_.reset(new btStaticPlaneShape(btVector3(0,1,0),1));
    ground_motion_state_.reset(new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0))));
    btRigidBody::btRigidBodyConstructionInfo ground_rigid_body_ci(0,ground_motion_state_.get(),ground_shape_.get(),btVector3(0,0,0));
    ground_rigid_body_.reset(new btRigidBody(ground_rigid_body_ci));
    dynamics_world_->addRigidBody(ground_rigid_body_.get());
}

physics_manager_t::~physics_manager_t()
{
    logger::log(DEBUG, "[Physics] Physics system destruction...");

    stop();

    //remove ground model
    dynamics_world_->removeRigidBody(ground_rigid_body_.get());
}


void physics_manager_t::start()
{
    logger::log(DEBUG, "[Physics] Starting system.");

    //create render in separate thread
    simulation_thr_ = boost::thread(&physics_manager_t::simulation_thr, this);
}

void physics_manager_t::stop()
{
    logger::log(DEBUG, "[Physics] Stopping system.");

    simulation_thr_.interrupt();

    if (simulation_thr_.joinable())
    {
        simulation_thr_.join();
    }
}

void physics_manager_t::add_simulation_object(boost::uint64_t ID, boost::shared_ptr<motion_state_t> motion_state, vector3_t &bounding_box, scalar_t mass)
{
    boost::shared_ptr<simulation_object_t> ret(new simulation_object_t);

    ret->motion_state = motion_state;
    ret->shape.reset(new btBoxShape(btVector3(bounding_box.x * (scalar_t)0.5, bounding_box.y * (scalar_t)0.5, bounding_box.z * (scalar_t)0.5)));

    btVector3 inertia(0, 0, 0);

    ret->shape->calculateLocalInertia(mass, inertia);

    btRigidBody::btRigidBodyConstructionInfo rigid_body_ci(mass, motion_state.get(), ret->shape.get(), inertia);

    ret->rigid_body.reset(new btRigidBody(rigid_body_ci));

    dynamics_world_->addRigidBody(ret->rigid_body.get());

    sim_obj_map_[ID] = ret;
}

void physics_manager_t::remove_simulation_object(boost::uint64_t ID)
{
    sim_obj_map_t::iterator it = sim_obj_map_.find(ID);
    assert( it != sim_obj_map_.end() );
    dynamics_world_->removeRigidBody(it->second->rigid_body.get());
    sim_obj_map_.erase(it);
}

void physics_manager_t::simulation_thr()
{
    logger::thr_desc_t name_thr("physics");

    logger::log(DEBUG, "[Physics] Starting simulation loop thread.");

    engine_->report_started();

    engine_->wait_all_systems();

    try
    {

        const static boost::uint32_t ms_to_wait_src = 10;

        const static boost::posix_time::milliseconds ms_to_wait(ms_to_wait_src);

        const static btScalar sim_step = ms_to_wait_src/1000.f;

        while (true)
        {
            {
                boost::mutex::scoped_lock g(engine_->system<object_manager_t>().objects_sync_locker_);

                boost::this_thread::interruption_point();

                dynamics_world_->stepSimulation(sim_step, 10);
            }
            boost::this_thread::sleep(ms_to_wait);
        }

    }
    catch (boost::thread_interrupted)
    {
    }

    logger::log(DEBUG, "[Physics] Physics simulation thread interrupted normally.");
}

